#ifndef __CONTROLLER_BASE_H__
#define __CONTROLLER_BASE_H__

#include <ros/ros.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/PoseStamped.h>
#include <sensor_msgs/PointCloud2.h>
#include <nav_msgs/Path.h>

#include "map_base.h"
#include "core/common/core.h"
#include "core/convert/msg_convert.h"
#include "core/utils/graphis_utils.h"
#include "core/utils/math_utils.h"
#include "path_generate/path_base.h"
#include "path_generate/p2p/jps.h"
#include "path_generate/p2p/astar.h"
using namespace std;

// 控制器类型
enum ContrllerType{
    DWA = 1,
    Pure_Pursuit = 2,
    PID = 3,
    MPC = 4,
};

// 控制器默认全局配置参数
struct ContrllerBaseConfig{
    double max_vel_x = 0.5;
    double min_vel_x = 0.0;
    double max_vel_yaw = 1.0;
    double min_vel_yaw = -1.0;
    double linear_acc = 0.5;
    double angular_acc = 1.0;
    double goal_distance_tolerance = 0.1;
    double goal_angle_tolerance = utils::math::toRad(10);
    double lookahead_time = 1.5;
    double min_lookahead_distance = 0.5;
    double max_lookahead_distance = 1.5;
    double avoidance_distance = 1.0;    //避障距离
    int controller_frequency = 20;  //20Hz
};

// struct WayPoint{
//     geometry_msgs::PoseStamped posestamp;
//     // 速度信息
//     double vx = 0.0;
//     double vy = 0.0;
//     double vth = 0.0;
//     // 曲率
//     double curvature = 0.0;
// };

// typedef vector<WayPoint> TrajectoryPoints;

class ControllerBase{
    public:
        ControllerBase();
        ~ControllerBase() = default;

        /*************************************************
        * 虚函数
        *************************************************/
        // 计算速度
        // virtual mc::status computeTwist(geometry_msgs::PoseStamped goal, geometry_msgs::Twist curr_vel,
        //     geometry_msgs::Twist& cmd_vel) = 0;
        virtual mc::status computeTwist(mc::TrajectoryPoint2D goal, geometry_msgs::Twist curr_vel,
            geometry_msgs::Twist& cmd_vel) = 0;

        /*************************************************
        * 数据同步
        *************************************************/
        // 同步代价地图 
        void updateCostmapData(boost::shared_ptr<nav_msgs::OccupancyGrid> costmap_ptr);
        // 同步机器人位姿
        void updateRobotPose(geometry_msgs::PoseStamped robot_pose);
        // 同步原始全局路径
        void updateGlobalPath(vector<geometry_msgs::PoseStamped> path);
        // 获取实时全局路径 
        vector<geometry_msgs::PoseStamped> getGlobalPath();
        
        /*************************************************
        * 位姿转换
        *************************************************/
        // 获取TF位姿
        bool getTFPose(tf2_ros::Buffer* tf_buffer, geometry_msgs::PoseStamped& global_pose, 
            string father_frame, string child_frame, ros::Time stamp);
        // 获取机器人当前位姿
        bool getRobotPose(tf2_ros::Buffer* tf_buffer, geometry_msgs::PoseStamped& global_pose);
        // 全局位姿转局部
        // geometry_msgs::PoseStamped convertGlobalPoseToLocal(geometry_msgs::PoseStamped global_pose, 
        //     geometry_msgs::PoseStamped robot_pose);
        mc::TrajectoryPoint2D convertGlobalPoseToLocal(mc::TrajectoryPoint2D global_pose, mc::TrajectoryPoint2D robot_pose);
        // 局部位姿转全局
        // geometry_msgs::PoseStamped convertLocalPoseToGlobal(geometry_msgs::PoseStamped local_pose, 
        //     geometry_msgs::PoseStamped robot_pose);
        mc::TrajectoryPoint2D convertLocalPoseToGlobal(mc::TrajectoryPoint2D global_pose, mc::TrajectoryPoint2D robot_pose);

        /*************************************************
        * 辅助函数
        *************************************************/
        // 旋转至目标角度
        // void rotateToTGoal(geometry_msgs::PoseStamped robot_pose,
        //     geometry_msgs::PoseStamped goal_pose, geometry_msgs::Twist& cmd_vel);
        void rotateToTGoal(mc::TrajectoryPoint2D robot_pose,
            mc::TrajectoryPoint2D goal_pose, geometry_msgs::Twist& cmd_vel);
        // 计算最大减速距离
        double maxSpeedToStop(double vel, double acc);
        // 计算直线刹车距离 v^2 - 0 = 2as
        double maxStopDistance(geometry_msgs::Twist cmd_vel, double acc);
        
        // 检测目标点是否到达  距离判断
        // bool isPositionReached(geometry_msgs::PoseStamped robot_pose, geometry_msgs::PoseStamped goal_pose);
        bool isPositionReached(mc::TrajectoryPoint2D robot_pose, mc::TrajectoryPoint2D goal_pose);
        // 检测目标点是否到达  角度判断
        // bool isAngleReached(geometry_msgs::PoseStamped robot_pose, geometry_msgs::PoseStamped goal_pose);
        bool isAngleReached(mc::TrajectoryPoint2D robot_pose, mc::TrajectoryPoint2D goal_pose);
        // 检测目标点是否到达
        // bool isGoalReached(geometry_msgs::PoseStamped robot_pose, geometry_msgs::PoseStamped goal_pose);
        bool isGoalReached(mc::TrajectoryPoint2D robot_pose, mc::TrajectoryPoint2D goal_pose);

        
        /*************************************************
        * 路径处理
        *************************************************/   
        // 轨迹裁剪
        void pruningPath(double dis);  
        // 获取前瞻距离
        double getLookAheadDistance(double vt);   
        // 获取前瞻目标点
        bool getLookaheadGoal(double lookahead_distance, mc::TrajectoryPoint2D& lookahead_goal, int& index);
        // 获取距离最近的点
        bool getClosedPose(int& closest_index);
        // 校验当前点到目标点之间是否存在障碍物
        bool isPathCollision(int& collision_index);
        // 路径重规划
        bool replanPath(int collision_index);

        /*************************************************
        * 速度控制
        *************************************************/
        // 急停控制
        void e_stop();  
        // 减速控制
        void slow_down(double controller_frequency);
        // 停止控制
        void sendEmptyTwist();
        // 发送速度
        void sendVelocity(geometry_msgs::Twist& cmd_vel);

        // 符号判断
        inline float sign(float x){
            return x < 0.0 ? -1.0 : 1.0;
        }        

        protected:
            MapBase map_base_;
            ContrllerBaseConfig baseConfig_;
            geometry_msgs::Twist cmd_vel_;                   // 当前速度
            boost::shared_ptr<PlannerBase> path_planner_ptr_;
            // geometry_msgs::PoseStamped robot_posestamp_;
            mc::TrajectoryPoint2D robot_pose_;
            mc::Trajectory2D origin_trajectory_;            // 原始轨迹点
            mc::Trajectory2D trajectory_points_;            // 优化后的轨迹点
            string baseFrameId_;
            
            mc::TrajectoryROS trajectoryROS_;

        private:
            ros::NodeHandle nh_;
            ros::Publisher speedPub_;
};

#endif